# -*- coding: utf-8 -*-
"""
Created on Tue Apr 11 22:09:31 2023

@author: Administrator
"""

'''

编写：ZNZZ  执念执战 WCC
QQ: 572314251
blogs: https://home.cnblogs.com/u/zhinianzhizhan/

'''



import  ArmControl_MainUI

import PyQt5
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import QApplication, QMainWindow,QDialog
from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from PyQt5.QtGui import *

import sys



import math
import MyArmTest 

 


class MyLabel(QLabel):
    '''
    重写label，用于获取位置调用
    '''

    def __init__(self, parent=QLabel):
        super().__init__(parent)
        self.setMouseTracking(True)  # 开启鼠标追踪功能
        self.setGeometry(0,0,660,440)#设置大小，设置成和输入label一致的大小，覆盖掉原来的label,可以更改为传递大小的
        

        
        
    def mouseMoveEvent(self, e):
        '''
        鼠标移动触发的回调事件
        名字是固定的
        '''

        x = e.localPos().x() #获取控件内的相对坐标。注意与父类label中的点击事件获取的坐标的区分
        y = e.localPos().y()
        
        MoveX,MoveY =round((x-330)/660*100,2), round((y-110)/330*50,2)
        if math.sqrt(MoveX*MoveX+MoveY*MoveY) >50.0:
            text = "Out of ArmRange"
        elif math.sqrt(MoveX*MoveX+MoveY*MoveY) <10.0:
            text = "Out of ArmRange"
        else:
            text = 'x: {0}, y: {1}'.format(round((x-330)/660*100,2), round((y-110)/330*50,2))

        MainWindow.MouseMoveLabel.setText(text)   
        MainWindow.MouseMoveLabel.adjustSize() # 自适应宽度
        MainWindow.MouseMoveLabel.move((int)(e.localPos().x()),(int)(e.localPos().y()))  ##移动用于显示的label到鼠标位置，实现跟随显示
        MainWindow.InforShowLable.setText(text)#左下角的label也显示一下
        
  







class MainWindowClass(QDialog):
    '''
    主窗体
    注意，之前使用 QMainWindow ，只有鼠标按下时才会触发 mouseMoveEvent 事件（包括在laibel中），setMouseTracking(True)也没用。
    换为QDialog 就没问题了。
    '''
    def __init__(self):
        super().__init__()
        
        self.main_ui = ArmControl_MainUI.Ui_Dialog()
        self.main_ui.setupUi(self)#绑定
        self.setMouseTracking(True)#鼠标追踪，
        

        self.label_GetPos = MyLabel(self.main_ui.label_GetPos)  #绑定重写的label子类
        self.label_GetPos.setMouseTracking(True)#鼠标追踪   
        
        
        self.png = QPixmap('ArmRange.png') #画布 背景图 画笔等
            #最新更新，找不到没有修改的背景图了，不想再截图了，就用这个吧，有之前手动画的圆，也有最新绘制的圆，显得很乱...
        print("png Size:",self.png.width,self.png.height)
        self.pngcopy = self.png.copy() #复制一份背景图用于覆盖，因为画笔画完后会保留，所以覆盖原图实现画笔清空
        self.label_GetPos.setPixmap(self.pngcopy) #设置背景图       
        #self.painter = QPainter(self.pngcopy)
        
     
        self.MouseMoveLabel = self.main_ui.label_MouseMoveShow
        self.MouseMoveLabel.adjustSize() # 自适应宽度
        #self.MouseMoveLable.setAlignment(Qt.AlignCenter)
        
        
        self.InforShowLable = self.main_ui.label_PosInfor
        
        

        self.mouseX_Pos = 0
        self.mouseY_Pos = 0
        
        self.labelX_Pos = 0
        self.labelY_Pos = 0

        self.ArmCenterX = 330
        self.ArmCenterY = 110


        #连接到机械臂
        self.Arm = MyArmTest.MyArmBasicClass()
        self.ArmconnectedRet = self.Arm.ConnectedStart()
        if  self.ArmconnectedRet == 0: #连接成功
            self.Arm.GetArmHandle()
            self.Arm.GetShapeBasePosition()#获取底座坐标
            position1 = self.Arm.GetJointAngle("ArmJoint1")  #获取三个关节的角度/位置
            position2 = self.Arm.GetJointAngle("ArmJoint2")
            position1 = self.Arm.GetJointAngle("BaseJoint")
            self.Arm.SetJointAngle("ArmJoint1",0) #设置回默认角度
            self.Arm.SetJointAngle("ArmJoint2",0)
            self.Arm.SetJointAngle("ArmJoint3",0)
            
            
            
        else:
            
            print("Connected Error!! Pleace check and retry!!")
            print("需要先在  coppeliasim 软件中开启仿真，再开始本python才能正确远程连接！！")
 
    
    def DrawTeest(self):
        self.painter.drawLine(0,0,400,400)
        self.label_GetPos.setPixmap(self.png) #设置背景图      

    
    def enterEvent (self,evn):
        '''
        主窗体的鼠标移入事件，更改鼠标图标

        Parameters
        ----------
        evn : TYPE
            DESCRIPTION.

        Returns
        -------
        None.

        '''
        print("鼠标移入主界面")
        self.setCursor( QtCore.Qt.CrossCursor)  #改变指针
    def leaveEvent (self, evn):
        '''
        主窗体的鼠标移出事件，更改鼠标图标

        Parameters
        ----------
        evn : TYPE
            DESCRIPTION.

        Returns
        -------
        None.

        '''
        print("鼠标移出主界面")  
        self.setCursor( QtCore.Qt.ArrowCursor)
        
    def mouseMoveEvent(self,evn):
        return
        print("mouseMoveEvent")
       # print(evn.globalPos())
        
        
    def mouseDoubleClickEvent(self, evn):
        print("mouseDoubieClickEvent")
    def mouseClickEvent(self, evn):
        print("mouseClickEvent")    
    def mouseReleaseEvent(self, evn):
        '''
        主窗体的鼠标释放事件。以鼠标释放视为电机完毕，惊醒下一步操作

        Parameters
        ----------
        evn : TYPE
            DESCRIPTION.

        Returns
        -------
        None.

        '''
        print("鼠标释放")  # 响应测试语句   
        
        x,y= evn.localPos().x(),evn.localPos().y()  #注意和 MyLabel 类中的获取区分。这个localPos在此处获取的是相对窗口的坐标，MyLabel 因为本身绑定的是label，
                #所以直接获取到相对label的坐标，但是此处绑定的只有背景窗口，所以获取的是相对于窗口的坐标，还要减去label的左上角坐标才是相对于label的坐标        
        x = x -self.main_ui.label_GetPos.x() #我们需要的是label内相对坐标，所以要减去label的左上角坐标
        y = y -self.main_ui.label_GetPos.y()
        print("Click Pos:",x," ",y)
        print("label pos:",self.main_ui.label_GetPos.x()," ",self.main_ui.label_GetPos.y())
        MoveX,MoveY =round((x-330)/660*100,4), round((y-110)/330*50,4)  #这些计算是把label内获取的鼠标坐标换算为半径为50cm的半径的圆的坐标
        print("Clicked Pos: X:",MoveX," Y:",MoveY)
        if math.sqrt(MoveX*MoveX+MoveY*MoveY) >50.0:
            print( "Out of ArmRange")
            return 
        elif math.sqrt(MoveX*MoveX+MoveY*MoveY) <10.0:
            print("Out of ArmRange")
            return

        
        
        
        if  self.ArmconnectedRet == 0:
            Point = self.Arm.CalPointOfSection1(0,0,30,MoveX,MoveY,20) #计算交点坐标
            print("CalPointOfSection:")
            print(Point.Point1,Point.Point2)
            self.pngcopy = self.png.copy()#复制一份背景图用于覆盖，因为画笔画完后会保留，所以覆盖原图实现画笔清空
            self.painter = QPainter(self.pngcopy)



            #画出XY 坐标轴
            self.painter.drawLine(0,110,660,110)
            self.painter.drawLine(330,0,330,440)
            pen = QPen(Qt.blue,Qt.SolidLine)
            self.painter.setPen(pen)
            #以下是连接交点与臂两端的端点,即画两个机械臂
            self.painter.drawLine(self.ArmCenterX,self.ArmCenterY,int(Point.Point1[0]/100*660+330),int(Point.Point1[1]/50*330+110))
            self.painter.drawLine(self.ArmCenterX,self.ArmCenterY,int(Point.Point2[0]/100*660+330),int (Point.Point2[1]/50*330+110))
            self.painter.drawLine(int(Point.Point1[0]/100*660+330),int(Point.Point1[1]/50*330+110),int(x),int(y))
            self.painter.drawLine(int(Point.Point2[0]/100*660+330),int(Point.Point2[1]/50*330+110),int(x),int(y))
            #绘制参考辅助线
            pen = QPen(Qt.white,Qt.DashLine)
            self.painter.setPen(pen)
            self.painter.drawLine(int(Point.Point1[0]/100*660+330),int(Point.Point1[1]/50*330+110),int(Point.Point2[0]/100*660+330),int(Point.Point2[1]/50*330+110))#绘制两个交点之间的连接辅助线
            self.painter.drawLine(self.ArmCenterX,self.ArmCenterY,int(x),int(y))#绘制目标点和原点之间的辅助线
            
            
            #将坐标在坐标位置在label中显示出来
            self.painter.setFont(QFont("simsun",10))
            self.painter.drawText(QPoint(int(Point.Point1[0]/100*660+330),int(Point.Point1[1]/50*330+110)),"("+str(round(Point.Point1[0],2))+","+str(round(Point.Point1[1],2))+")")
            self.painter.drawText(QPoint(int(Point.Point2[0]/100*660+330),int(Point.Point2[1]/50*330+110)),"("+str(round(Point.Point2[0],2))+","+str(round(Point.Point2[1],2))+")")
            self.painter.drawText(QPoint(int(x),int(y)),"("+str(round(MoveX,2))+","+str(round(MoveY,2))+")")


            pen = QPen(Qt.yellow,Qt.DashLine)
            self.painter.setPen(pen)
            ArcR=int(10/100*660)
           
            self.painter.drawEllipse(QPoint(self.ArmCenterX,self.ArmCenterY),ArcR,ArcR) #中心10cm的超范围区
            
            pen = QPen(Qt.red,Qt.DashLine)
            self.painter.setPen(pen)
            ArcR=int(30/100*660)
           
            self.painter.drawEllipse(QPoint(self.ArmCenterX,self.ArmCenterY),ArcR,ArcR)#第一长臂所在圆
            pen = QPen(Qt.green,Qt.DashLine)
            self.painter.setPen(pen)
            ArcR=int(20/100*660)
            self.painter.drawEllipse(QPoint(int(x),int(y)),ArcR,ArcR)#第二短臂所在圆

            
            
            
            
            pen = QPen(Qt.black,Qt.DashLine)
            self.painter.setPen(pen)
            ArcR=int(50/100*660)
            self.painter.drawEllipse(QPoint(self.ArmCenterX,self.ArmCenterY),ArcR,ArcR)#臂展最大50cm范围
            
            self.label_GetPos.setPixmap(self.pngcopy) #设置背景图    
            self.painter = 0
            
            
            
            
            #下面是计算角度，用于coppeliasim中进行旋转
            angleGoalPoint = self.Arm.CalAngleOfPoint1(0,0,MoveX,MoveY,math.sqrt(MoveX*MoveX+MoveY*MoveY)) #计算鼠标点击的目标坐标与0点的连线相对于Y轴坐标的方向
            print("点击坐标与原点连线相对于Y轴的角度： ",angleGoalPoint)
            angleTwoPoint = self.Arm.CalAngleBetweenTwoPoint(Point.Point1[0],Point.Point1[1],Point.Point2[0],Point.Point2[1],30)
            print("两个计算交点与原点连线夹角：",angleTwoPoint)           
            angle1 = round(angleGoalPoint-angleTwoPoint/2,4)#计算臂一实际的旋转角度
            print("Moveangle1：",angle1)            
            angle2 = self.Arm.CalAngleOfArm2(Point.Point1[0],Point.Point1[1],30,MoveX,MoveY,20,angleTwoPoint/2)#计算臂二的旋转角度
            print("Moveangle2：",angle2)      
            self.Arm.SetJointAngle("ArmJoint1",angle1)
            self.Arm.SetJointAngle("ArmJoint2",90-angle2)#臂二要在臂一的基础上旋转，所以要加上臂一的旋转角度
            print(Point.PointNum,Point.Point1,Point.Point2)    
            

            
            
           
        
    def MainWindow_Show(self):
        self.ThisMainWindow.show()
        
        
    def MainWindow_pushButton_MoveBack(self):
        print("pushButton_MoveBack Click!")
        
    
        
        
        

MainWindow = MainWindowClass();
        
        
def main():
    MainWindow.show()
    
    
    
    
    
if __name__ == '__main__':
    app=QApplication(sys.argv)
    main()
   